#include "arith_pid.h"

#define DBG_TAG    "algo_pid"
#define DBG_LVL    DBG_INFO
#include <rtdbg.h>

#define PID_WORK_PERIOD               100   //PID工作周期

void pid_normal(rt_uint16_t *pidConfig, rt_int32_t sv, rt_int32_t pv, rt_int16_t *output)
{
    float outputKp;
    static float outputKi;
    float outputKd;
    static rt_int32_t preTempEk = 0;
    rt_int32_t threshold = 100000;
    float kp = (rt_uint16_t)pidConfig[0] / 10000.0f;
    float ki = (rt_uint16_t)pidConfig[1] / 10000.0f;
    float kd = (rt_uint16_t)pidConfig[2] / 10000.0f;
    rt_int16_t outputSum;
    rt_int32_t tempEk = sv - pv;
    
    if(tempEk > threshold || tempEk < -threshold) //偏差过大，全功率输出
    {
        (tempEk > threshold) ? (outputSum = 1000) : (outputSum = -1000);
    }
    else
    {
        outputKp = kp * tempEk;
        outputKi += ki * tempEk;
        outputKd = kd * (tempEk - preTempEk);

        if(outputKi > 1000)     //积分限幅
            outputKi = 1000;
        if(outputKi < -1000)
            outputKi = -1000;
#if SYSTEM_IS_ROUND_OFF
        outputSum = outputKp + outputKi + outputKd + 0.5f;
#else
        outputSum = outputKp + outputKi + outputKd;
#endif
        if(outputSum > 1000)     //总和限幅
            outputSum = 1000;
        if(outputSum < -1000)
            outputSum = -1000;

        preTempEk = tempEk;
    }   
    *output = outputSum;
    // rt_kprintf("sv = %d, pv = %d, ek = %d, outKp = %d, outKi = %d, outKd = %d, outSum = %d\n", sv, pv, tempEk, (rt_int32_t)outputKp, (rt_int32_t)outputKi, (rt_int32_t)outputKd, (rt_int32_t)outputSum);
}

rt_uint8_t pid_at(rt_uint16_t *pidConfig, rt_int32_t sv, rt_int32_t pv, rt_int16_t *output)
{
    static rt_uint8_t atStep;       //状态机运行过程
    static rt_uint8_t atMode;       //0：先制冷     1：先制热
    static rt_int32_t minValue;     //最小值
    static rt_int32_t maxValue;     //最小值
    static rt_uint8_t crossZeroCnt; //过零次数
    static rt_uint32_t cntStart;    //开始计算周期时的AT周期数
    static rt_uint32_t cntEnd;      //结束计算周期时的AT周期数
    static rt_uint32_t periodCnt;   //总AT周期数
    rt_uint8_t ret = 0;

    periodCnt++;
 
    switch (atStep)                 // AT状态机
    {
    case 0:                         // 根据当前偏差作相应处理
        if (pv < sv)    // 加热
        {
            *output = 1000;      
            atStep = 1;
            atMode = 1;
        }
        else
        {
            *output = -1000;       //制冷
            atStep = 2;
            atMode = 0;
        }
        minValue = sv;
        maxValue = sv;
        break;

    case 1:     //TEC全功率加热直至超调
        if(pv < sv)
        {
            *output = 1000;
            if(atMode == 0)
            {
                if(crossZeroCnt == 3)
                {
                    if(minValue > pv)
                    {
                        minValue = pv;
                    }
                }
            }
            else if(atMode == 1)
            {
                if(crossZeroCnt == 2)
                {
                    if(minValue > pv)
                    {
                        minValue = pv;
                    }
                }
            }

        }
        else
        {
            if(++crossZeroCnt < 4)  //未完成整定
            {
                if(crossZeroCnt == 2)
                {
                    cntStart = periodCnt;
                }
                atStep = 2;
                *output = -1000;     //全功率制冷
            }
            else    //整定完成
            {
                cntEnd = periodCnt;
                atStep = 3;
            }
        }
        break;

    case 2:     //TEC全功率制冷直至超调
        if(pv > sv)
        {
            *output = -1000;
            if(atMode == 0)
            {
                if(crossZeroCnt == 2)
                {
                    if(maxValue < pv)
                    {
                        maxValue = pv;
                    }
                }
            }
            else if(atMode == 1)
            {
                if(crossZeroCnt == 3)
                {
                    if(maxValue < pv)
                    {
                        maxValue = pv;
                    }
                }
            }
        }
        else
        {
            if(++crossZeroCnt < 4)  //未完成整定
            {
                if(crossZeroCnt == 2)
                {
                    cntStart = periodCnt;
                }
                atStep = 1;
                *output = 1000; //全功率制热
            }
            else    //整定完成
            {
                cntEnd = periodCnt;
                atStep = 3;
            }
        }
        break;

    case 3:     //根据上述采集的数据计算PID参数
        {
            float Kc = 127.0f / (maxValue - minValue);
            float Tc = (cntEnd - cntStart) * PID_WORK_PERIOD / 1000.0f;
            float kp = 0.6f * Kc * 10;  
            float ki = (0.6f * Kc) / (0.5f * Tc);
            float kd = (0.6f * Kc) * 0.125f * Tc * 10;
            pidConfig[0] = (rt_uint16_t)(kp * 10000); /*上传PID参数*/
            pidConfig[1] = (rt_uint16_t)(ki * 10000);
            pidConfig[2] = (rt_uint16_t)(kd * 10000);
            periodCnt = 0;
            atStep = 0;
            crossZeroCnt = 0;
            ret = 1;
            break;
        }
    }
    return ret;
}

